package com.jmex.physics.impl.jbullet.replacementShape;

import javax.vecmath.Vector3f;

import com.bulletphysics.BulletGlobals;
import com.bulletphysics.collision.broadphase.BroadphaseNativeType;
import com.bulletphysics.collision.shapes.CapsuleShape;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.linearmath.VectorUtil;

/**
 * 
 * This is a hack to try to get the JBullet capsule shape to match up with
 * the JMEPhysics PhysicsCapsule.  For starters, JBullet capsules are generated
 * around the Y axis, rather than Z.  Secondly, for some reason the scaling
 * doesn't match up.  A JBullet CapsuleShape seems to be exactly 1/2 the width,
 * height, and depth of a corresponding JMEPhysics PhysicsCapsule.
 * 
 * I've chosen to compensate for the scaling problem in the JBulletCapsule
 * class by simply creating a capsule 2x the size.  This solution may need
 * to be revisited.
 * 
 * @author Falken224
 *
 */

public class JMECompatibleCapsuleShape extends CapsuleShape {
	public JMECompatibleCapsuleShape(float radius, float height) {
		super(radius,height);
		implicitShapeDimensions.set(radius, radius, 0.5f * height);
	}

	@Override
	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0) {
		stack.vectors.push();
		try {
			Vector3f supVec = stack.vectors.get(0f, 0f, 0f);

			float maxDot = -1e30f;

			Vector3f vec = stack.vectors.get(vec0);
			float lenSqr = vec.lengthSquared();
			if (lenSqr < 0.0001f) {
				vec.set(1f, 0f, 0f);
			}
			else {
				float rlen = 1f / (float) Math.sqrt(lenSqr);
				vec.scale(rlen);
			}

			Vector3f vtx = stack.vectors.get();
			float newDot;

			float radius = getRadius();

			Vector3f tmp1 = stack.vectors.get();
			Vector3f tmp2 = stack.vectors.get();

			{
				Vector3f pos = stack.vectors.get(0f, 0f, getHalfHeight());
				VectorUtil.mul(tmp1, vec, localScaling);
				tmp1.scale(radius);
				tmp2.scale(getMargin(), vec);
				vtx.add(pos, tmp1);
				vtx.sub(tmp2);
				newDot = vec.dot(vtx);
				if (newDot > maxDot) {
					maxDot = newDot;
					supVec.set(vtx);
				}
			}
			{
				Vector3f pos = stack.vectors.get(0f, 0f, -getHalfHeight());
				VectorUtil.mul(tmp1, vec, localScaling);
				tmp1.scale(radius);
				tmp2.scale(getMargin(), vec);
				vtx.add(pos, tmp1);
				vtx.sub(tmp2);
				newDot = vec.dot(vtx);
				if (newDot > maxDot) {
					maxDot = newDot;
					supVec.set(vtx);
				}
			}

			return stack.vectors.returning(supVec);
		}
		finally {
			stack.vectors.pop();
		}
	}

	@Override
	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
		// TODO: implement
		throw new UnsupportedOperationException("Not supported yet.");
	}

	@Override
	public void calculateLocalInertia(float mass, Vector3f inertia) {
		stack.pushCommonMath();
		try {
			// as an approximation, take the inertia of the box that bounds the spheres

			Transform ident = stack.transforms.get();
			ident.setIdentity();

			float radius = getRadius();

			Vector3f halfExtents = stack.vectors.get(radius, radius, radius + getHalfHeight());

			float margin = BulletGlobals.CONVEX_DISTANCE_MARGIN;

			float lx = 2f * (halfExtents.x + margin);
			float ly = 2f * (halfExtents.y + margin);
			float lz = 2f * (halfExtents.z + margin);
			float x2 = lx * lx;
			float y2 = ly * ly;
			float z2 = lz * lz;
			float scaledmass = mass * 0.08333333f;

			inertia.x = scaledmass * (y2 + z2);
			inertia.y = scaledmass * (x2 + z2);
			inertia.z = scaledmass * (x2 + y2);
		}
		finally {
			stack.popCommonMath();
		}
	}

	@Override
	public BroadphaseNativeType getShapeType() {
		return BroadphaseNativeType.CAPSULE_SHAPE_PROXYTYPE;
	}

	@Override
	public String getName() {
		return "CapsuleShape";
	}
	
	public float getRadius() {
		return implicitShapeDimensions.x;
	}

	public float getHalfHeight() {
		return implicitShapeDimensions.z;
	}

}
